import math

class NonlinearSteerController:
    def __init__(self, param: dict):
        self.L = param.get("L", 0.9)
        self.alpha1 = param.get("alpha1", 0.95)
        self.beta1  = param.get("beta1", 1.05)
        self.c1 = param.get("c1", 0.2)
        self.c2 = param.get("c2", 0.2)
        self.k1 = param.get("k1", 1.1)
        self.k2 = param.get("k2", 0.18)
        self.set_region = param.get("set_region", 0.1)
        self.v_small_for_region = param.get("v_small_for_region", 0.01)
        self.K_max = param.get("K_max", 0.2)
        self.p = param.get("p", 1.1)
        self.q = param.get("q", 0.9)
        self.tanh_scale = param.get("tanh_scale", 0.1)
        self.integral = 0.0  # 内部积分累加器

    @staticmethod
    def sign(x: float) -> float:
        return 1.0 if x > 0 else (-1.0 if x < 0 else 0.0)

    def step(self, *, lat_err: float, yaw_err: float, v: float, dt: float) -> float:
        trans = 180.0 / math.pi   # 单位保持原样,效果不好后面调；
        x1 = float(lat_err)
        x2 = float(yaw_err)
        d_hat = 0.0

        # 指数参数
        alpha2 = 2.0 * self.alpha1 / (self.alpha1 + 1.0)
        beta2  = 2.0 * self.beta1  / (self.beta1  + 1.0)

        #先用“积分的当前值”计算 s（与 Matlab 一致；积分放到后面再更新）
        s = x2 + self.integral

        # 计算 K（保持你的逻辑）
        region_threshold = abs(self.set_region - self.v_small_for_region)
        if abs(s) < region_threshold:
            denom = self.set_region - abs(s)
            temp = self.K_max if denom == 0 else abs(s) / denom
            K = self.K_max if temp > self.K_max else float(temp)
        else:
            K = self.K_max

        # 非线性项 g1, g2 与 y0
        g1 = (abs(x1) ** self.alpha1) * self.sign(x1) + x1 + (abs(x1) ** self.beta1) * self.sign(x1)
        g2 = (abs(x2) ** alpha2) * self.sign(x2) + x2 + (abs(x2) ** beta2) * self.sign(x2)
        y0 = self.k1 * g1 + self.k2 * g2

        gama = (self.p + self.q) / 2.0 + (self.p - self.q) / 2.0 * self.sign(abs(s) - 1.0)
        yyy = -(self.c1 * (abs(s) ** gama) * self.sign(s))

        delta = -(
            0.9 * (y0 - yyy + self.c2 * math.tanh(s)) +
            K * trans * math.tanh(0.1 * s)          # 保留 0.1 的常数系数
        )

        # 积分顺序调整到最后：u3 <- u3 + y0*dt（与 Matlab 外部积分一致） ===
        if dt > 0.0:
            self.integral += y0 * float(dt)

        return float(delta)